#!/bin/bash

# 检查参数数量最少为2个
if [ $# -lt 2 ]; then
    echo "用法: $0 <模块名> <操作编号>"
    echo "模块名可选: sensor 或 RMS"
    echo "操作编号说明:"
    echo "  1: 构建该模块"
    echo "  2: 启动发布者节点"
    echo "  3: 启动订阅者/监听者节点"
    exit 1
fi

MODULE=$1
ACTION=$2

# 处理 RMS 模块
if [ "$MODULE" == "rms" ]; then
    case $ACTION in
        1)
            echo "构建 rms_pkg 包..."
            colcon build --packages-select rms_pkg
            ;;
        2)
            echo "启动 RMS ..."
	    source install/setup.bash
            ./build/rms_pkg/RMS
	    ;;
        3)
            echo "启动 rms_pkg 节点..."
	    source install/setup.bash
            ros2 run rms_pkg rms_pkg --ros-args \
                -p timeout:=5 \
                --log-level DEBUG
            ;;
        *)
            echo "错误: RMS模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

# 处理 sensor 模块
elif [ "$MODULE" == "sensor" ]; then
    case $ACTION in
        1)
            echo "构建 sensor_cpp_pkg 包..."
            colcon build --packages-select sensor_cpp_pkg
            ;;
        2)
            echo "启动 sensor_hcd5014_publisher 节点..."
	    source install/setup.bash
            ros2 run sensor_cpp_pkg sensor_hcd5014_publisher --ros-args \
                -r __ns:=/robot1 \
                --log-level DEBUG
            ;;
        3)
            echo "启动 sensor_hcd5014_subscriber 节点..."
	    source install/setup.bash
            ros2 run sensor_cpp_pkg sensor_hcd5014_subscriber --ros-args \
                -r __ns:=/robot1 \
                --log-level DEBUG
            ;;
        *)
            echo "错误: sensor模块不支持的操作编号: $ACTION"
            exit 1
            ;;
    esac

# 模块名错误
else
    echo "错误: 不支持的模块名 '$MODULE'"
    echo "可用模块: RMS sensor"
    exit 1
fi

# 检查执行结果
if [ $? -ne 0 ]; then
    echo "操作失败！"
    exit 1
else
    echo "操作完成。"
fi
